package se.webbzon.oschi01.path3d;

import java.util.Collection;

import javax.vecmath.Point3d;

public class Path3D {
	
	// An array of all points included in this path
	private final Point3d[] points;
	
	// The length of the path
	private final double length;
	
	/*============================================================
	Constructors
	============================================================*/
	
	/** Creates a new 3d-path from an array of points. **/
	public Path3D(Point3d[] points) {
		this.points = points;
		double l = 0;
		for (int i = 0; i < points.length-1; i++) {
			l += points[i].distance(points[i+1]);
		}
		length = l;
	}
	
	/** Creates a new 3d-path from a collection of points. **/
	public Path3D(Collection<Point3d> points) {
		this(points.toArray(new Point3d[0]));
	}
	
	/*============================================================
	Public Methods
	============================================================*/
	
	/** Returns the x-position in the path at given position.
	 * The position must be in the range 0.0 (start) to 1.0 (end). **/
	public double getX(double position) {
		if (position <= 0 || points.length == 1)
			return points[0].x;
		if (position >= 1)
			return points[points.length-1].x;
		final double dp = 1 / (double) (points.length-1);
		final int index = (int) (position / dp);
		final double p = index * dp;
		final double xa = points[index].x, xb = points[index+1].x;
		return xa + (xb-xa)*(position-p)/dp;  
	}
	
	/** Returns the z-position in the path at given position.
	 * The position must be in the range 0.0 (start) to 1.0 (end). **/
	public double getY(double position) {
		if (position <= 0 || points.length == 1)
			return points[0].y;
		if (position >= 1)
			return points[points.length-1].y;
		final double dp = 1 / (double) (points.length-1);
		final int index = (int) (position / dp);
		final double p = index * dp;
		final double xa = points[index].y, xb = points[index+1].y;
		return xa + (xb-xa)*(position-p)/dp;  
	}
	
	/** Returns the z-position in the path at given position.
	 * The position must be in the range 0.0 (start) to 1.0 (end). **/
	public double getZ(double position) {
		if (position <= 0 || points.length == 1)
			return points[0].z;
		if (position >= 1)
			return points[points.length-1].z;
		final double dp = 1 / (double) (points.length-1);
		final int index = (int) (position / dp);
		final double p = index * dp;
		final double xa = points[index].z, xb = points[index+1].z;
		return xa + (xb-xa)*(position-p)/dp;  
	}
	
	/** Copies the coordinates from the path at a given position into a 
	 * Point3d instance. The position must be in the range 0.0 
	 * (start) to 1.0 (end). This method is more efficient than retrieving
	 * the x,y and z component separately. **/
	public void getPoint(double position, Point3d point) {
		if (position <= 0 || points.length == 1)
			point.set(points[0]);
		else if (position >= 1)
			point.set(points[points.length-1]);
		else {
			final double dp = 1 / (double) (points.length-1);
			final int index = (int) (position / dp);
			final double p = index * dp;
			point.interpolate(points[index], points[index+1], (position-p)/dp);
		}
	}
	
	/** Returns a position (in the range 0.0 to 1.0) which corresponds
	 * to the position in the path after traveling a given length
	 * from the start position. If the distance between all points
	 * is equal to 1 then this function will return length/getLength(). **/
	public double toPositon(double length) {
		final double dp = 1 / (double) (points.length-1);
		double l = 0;
		for (int i = 0; i < points.length-1; i++) {
			final double dl = points[i].distance(points[i+1]);
			if (l+dl > length) {
				return dp * (double) i + (length-l)/dl;
			}
			l += dl;
		}
		return 1;
	}
	
	/** Returns the length of the path. **/
	public double getLength() {
		return length;
	}

}
